/***************************************************************************//**
 * @file MSc_Matrix3x3.cpp
 * @author  Marek M. Cel <marekcel@mscsim.org>
 *
 * @section LICENSE
 *
 * Copyright (C) 2013 Marek M. Cel
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 ******************************************************************************/
#ifndef MSC_MATRIX3X3_CPP
#define MSC_MATRIX3X3_CPP
#endif

////////////////////////////////////////////////////////////////////////////////

#include <utils/MSc_Matrix3x3.h>

////////////////////////////////////////////////////////////////////////////////

MSc_Matrix3x3::MSc_Matrix3x3() :
    MSc_Matrix<3,3>()
{}

////////////////////////////////////////////////////////////////////////////////

MSc_Matrix3x3::MSc_Matrix3x3( const MSc_Matrix3x3 &mtrx ) :
    MSc_Matrix<3,3>()
{
    setArray( mtrx.m_items );
}

////////////////////////////////////////////////////////////////////////////////

MSc_Matrix3x3 MSc_Matrix3x3::getFrom( const double items[] )
{
    MSc_Matrix3x3 result;

    result.setArray( items );

    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Matrix3x3 MSc_Matrix3x3::getRotationMatrix( double phi, double tht, double psi )
{
    MSc_Matrix3x3 result;

    double sinPhi = sin( phi );
    double cosPhi = cos( phi );

    double sinTht = sin( tht );
    double cosTht = cos( tht );

    double sinPsi = sin( psi );
    double cosPsi = cos( psi );

    double sinPhiSinTht = sinPhi * sinTht;
    double cosPhiSinTht = cosPhi * sinTht;

    result.m_items[ 0 ] =  cosTht * cosPsi;
    result.m_items[ 1 ] =  cosTht * sinPsi;
    result.m_items[ 2 ] = -sinTht;

    result.m_items[ 3 ] = -( cosPhi * sinPsi ) + ( sinPhiSinTht * cosPsi );
    result.m_items[ 4 ] =  ( cosPhi * cosPsi ) + ( sinPhiSinTht * sinPsi );
    result.m_items[ 5 ] =  ( sinPhi * cosTht );

    result.m_items[ 6 ] =  ( sinPhi * sinPsi ) + ( cosPhiSinTht * cosPsi );
    result.m_items[ 7 ] = -( sinPhi * cosPsi ) + ( cosPhiSinTht * sinPsi );
    result.m_items[ 8 ] =  ( cosPhi * cosTht );

    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Matrix3x3 MSc_Matrix3x3::getRotationMatrix( const MSc_Angles &angl )
{
    return MSc_Matrix3x3::getRotationMatrix( angl.phi(), angl.tht(), angl.psi() );
}

////////////////////////////////////////////////////////////////////////////////

MSc_Matrix3x3 MSc_Matrix3x3::getRotationMatrix( const MSc_Quaternion &qtrn )
{
    MSc_Matrix3x3 result;

//    result.m_items[ 0 ] = pow2( qtrn.w() ) + pow2( qtrn.x() ) - pow2( qtrn.y() ) - pow2( qtrn.z() );
//    result.m_items[ 1 ] = 2.0 * ( qtrn.x()*qtrn.y() - qtrn.w()*qtrn.z() );
//    result.m_items[ 2 ] = 2.0 * ( qtrn.w()*qtrn.y() + qtrn.x()*qtrn.z() );

//    result.m_items[ 3 ] = 2.0 * ( qtrn.w()*qtrn.z() + qtrn.x()*qtrn.y() );
//    result.m_items[ 4 ] = pow2( qtrn.w() ) - pow2( qtrn.x() ) + pow2( qtrn.y() ) - pow2( qtrn.z() );
//    result.m_items[ 5 ] = 2.0 * ( qtrn.y()*qtrn.z() - qtrn.w()*qtrn.x() );

//    result.m_items[ 6 ] = 2.0 * ( qtrn.x()*qtrn.z() - qtrn.w()*qtrn.y() );
//    result.m_items[ 7 ] = 2.0 * ( qtrn.w()*qtrn.x() + qtrn.y()*qtrn.z() );
//    result.m_items[ 8 ] = pow2( qtrn.w() ) - pow2( qtrn.x() ) - pow2( qtrn.y() ) + pow2( qtrn.z() );

    // 0 1 2    0 3 6
    // 3 4 5    1 4 7
    // 6 7 8    2 5 8

    double w2 = qtrn.w()*qtrn.w();
    double x2 = qtrn.x()*qtrn.x();
    double y2 = qtrn.y()*qtrn.y();
    double z2 = qtrn.z()*qtrn.z();

    result.m_items[ 0 ] = w2 + x2 - y2 - z2;
    result.m_items[ 1 ] = 2.0 * ( qtrn.w()*qtrn.z() + qtrn.x()*qtrn.y() );
    result.m_items[ 2 ] = 2.0 * ( qtrn.x()*qtrn.z() - qtrn.w()*qtrn.y() );

    result.m_items[ 3 ] = 2.0 * ( qtrn.x()*qtrn.y() - qtrn.w()*qtrn.z() );
    result.m_items[ 4 ] = w2 - x2 + y2 - z2;
    result.m_items[ 5 ] = 2.0 * ( qtrn.w()*qtrn.x() + qtrn.y()*qtrn.z() );

    result.m_items[ 6 ] = 2.0 * ( qtrn.w()*qtrn.y() + qtrn.x()*qtrn.z() );
    result.m_items[ 7 ] = 2.0 * ( qtrn.y()*qtrn.z() - qtrn.w()*qtrn.x() );
    result.m_items[ 8 ] = w2 - x2 - y2 + z2;

    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Matrix3x3 MSc_Matrix3x3::getTransposed() const
{
    MSc_Matrix3x3 result;

    for ( int r = 0; r < m_rows; r++ )
    {
        for ( int c = 0; c < m_cols; c++ )
        {
            result.m_items[ c*m_cols + r ] = m_items[ r*m_cols + c ];
        }
    }

    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Matrix3x3& MSc_Matrix3x3::operator= ( const MSc_Matrix3x3 &mtrx )
{
    setArray( mtrx.m_items );
    
    return (*this);
}

////////////////////////////////////////////////////////////////////////////////

MSc_Matrix3x3 MSc_Matrix3x3::operator+ ( const MSc_Matrix3x3 &mtrx ) const
{
    MSc_Matrix3x3 result;

    for ( int i = 0; i < m_size; i++ )
    {
        result.m_items[ i ] = m_items[ i ] + mtrx.m_items[ i ];
    }
    
    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Matrix3x3 MSc_Matrix3x3::operator- ( const MSc_Matrix3x3 &mtrx ) const
{
    MSc_Matrix3x3 result;
    
    for ( int i = 0; i < m_size; i++ )
    {
        result.m_items[ i ] = m_items[ i ] - mtrx.m_items[ i ];
    }
    
    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Matrix3x3 MSc_Matrix3x3::operator* ( double val ) const
{
    MSc_Matrix3x3 result;
    
    for ( int i = 0; i < m_size; i++ )
    {
        result.m_items[ i ] = m_items[ i ] * val;
    }
    
    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Matrix3x3 MSc_Matrix3x3::operator* ( const MSc_Matrix3x3 &mtrx ) const
{
    MSc_Matrix3x3 result;

    for ( int r = 0; r < m_rows; r++ )
    {
        for ( int c = 0; c < m_cols; c++ )
        {
            result(r,c) = 0.0;

            for ( int i = 0; i < m_cols; i++ )
            {
                result(r,c) += ( m_items[ r*m_cols + i ] * mtrx(i,c) );
            }
        }
    }
    
    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Vector3 MSc_Matrix3x3::operator* ( const MSc_Vector3 &vect ) const
{
    MSc_Vector3 result;

    for ( int r = 0; r < m_rows; r++ )
    {
        result(r) = 0.0;

        for ( int c = 0; c < m_cols; c++ )
        {
            result(r) += ( m_items[ r*m_cols + c ] * vect(c) );
        }
    }
    
    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Matrix3x3 MSc_Matrix3x3::operator/ ( double val ) const
{
    MSc_Matrix3x3 result;
    
    for ( int i = 0; i < m_size; i++ )
    {
        result.m_items[ i ] = m_items[ i ] / val;
    }
    
    return result;
}

////////////////////////////////////////////////////////////////////////////////

MSc_Matrix3x3& MSc_Matrix3x3::operator+= ( const MSc_Matrix3x3 &mtrx )
{
    for ( int i = 0; i < m_size; i++ )
    {
        m_items[ i ] += mtrx.m_items[ i ];
    }
    
    return (*this);
}

////////////////////////////////////////////////////////////////////////////////

MSc_Matrix3x3& MSc_Matrix3x3::operator-= ( const MSc_Matrix3x3 &mtrx )
{
    for ( int i = 0; i < m_size; i++ )
    {
        m_items[ i ] -= mtrx.m_items[ i ];
    }
    
    return (*this);
}

////////////////////////////////////////////////////////////////////////////////

MSc_Matrix3x3& MSc_Matrix3x3::operator*= ( double val )
{
    for ( int i = 0; i < m_size; i++ )
    {
        m_items[ i ] *= val;
    }
    
    return (*this);
}

////////////////////////////////////////////////////////////////////////////////

MSc_Matrix3x3& MSc_Matrix3x3::operator/= ( double val )
{
    for ( int i = 0; i < m_size; i++ )
    {
        m_items[ i ] /= val;
    }
    
    return (*this);
}
